#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header

def publish_simulated_pose():
    # 初始化ROS节点
    rospy.init_node('simulated_pose_publisher', anonymous=True)
    
    # 创建一个发布者，发布到 /mavros/local_position/pose 话题
    pub = rospy.Publisher('/mavros/local_position/pose', PoseStamped, queue_size=10)
    
    # 设置发布频率（例如10Hz）
    rate = rospy.Rate(10)
    
    # 模拟的位置信息
    x, y, z = 0.0, 0.0, 1.0  # 初始位置 (x, y, z)
    yaw = 0.0  # 初始偏航角（弧度）
    
    while not rospy.is_shutdown():
        # 创建 PoseStamped 消息
        pose_msg = PoseStamped()
        
        # 设置消息头
        pose_msg.header = Header()
        pose_msg.header.stamp = rospy.Time.now()  # 当前时间
        pose_msg.header.frame_id = "map"  # 坐标系
        
        # 设置位置信息
        pose_msg.pose.position.x = x
        pose_msg.pose.position.y = y
        pose_msg.pose.position.z = z
        
        # 设置姿态信息（四元数）
        from tf.transformations import quaternion_from_euler
        quat = quaternion_from_euler(0, 0, yaw)  # 将欧拉角转换为四元数
        pose_msg.pose.orientation.x = quat[0]
        pose_msg.pose.orientation.y = quat[1]
        pose_msg.pose.orientation.z = quat[2]
        pose_msg.pose.orientation.w = quat[3]
        
        # 发布消息
        pub.publish(pose_msg)
        rospy.loginfo(f"Published simulated pose: x={x}, y={y}, z={z}, yaw={yaw}")
        
        # # 更新模拟位置（例如，让无人机沿x轴移动）
        y += 0.02  # 每次增加0.1米
        # yaw += 0.1  # 每次增加0.1弧度
        
        # 确保偏航角在 [0, 2π] 范围内
        if yaw > 6.28318530718:  # 2π
            yaw = 0.0
        
        # 等待下一个周期
        rate.sleep()

if __name__ == '__main__':
    try:
        publish_simulated_pose()
    except rospy.ROSInterruptException:
        pass

